//----------------------------------------------------------------------------
// M A R I T I M E  R E S E A R C H  I N S T I T U T E  N E T H E R L A N D S
//----------------------------------------------------------------------------
// Copyright (C) 2008 - MARIN - All rights reserved - http://www.marin.nl
//----------------------------------------------------------------------------
// Program    : mermaid
// Module     : vbm
// File       : Cg4Solver.cpp
//----------------------------------------------------------------------------

#include <math.h>       // sqrt
#include "Cg4Solver.h"
#include "FloatUtils.h" // square
#include "assertplus.h"

#ifdef _WINDOWS
#pragma warning(disable: 4244) // conversion from 'double' to 'float', possible loss of data
#endif

#define COLUMNS  (unsigned)(Nx1 + 3)
#define ROWS     (unsigned)(Nx2 + 3)

// cg4.cpp <- cg4.f90 with help of f2c and manually improved - MARIN MSG Anneke Sicherer-Roetman 20080526

//  =================================================================== 
//        Solver deel van de CG methode, voor beschikbare 
//        matrix-coefficienten a1, a2 en m_a3, alsmede preconditioner D 
//  =================================================================== 
//   
//           29 maart 1994, Auke van der Ploeg 
//   
//        Dit programma bepaalt de oplossing van Ax=B, met A een 
//        pentadiagonale NxN matrix die komt van een standaard 
//        discretisatie op een N*M-grid, waarbij N en M maximaal 
//        Mbg mag zijn. Dit betekent dat het aantal onbekenden N dus 
//        gelijk is aan N*M. 
//    
//    !!  Dit programma gebruikt de symmetrie van de matrix. 
//    
//        De tijdmeting wordt nu gedaan door het statement 
//         "start=dtime(rij2)". Op andere computers moet dit 
//        waarschijnlijk gewijzigd worden. 
//   
//  =================================================================== 
//           De iteratieve methode 
//  =================================================================== 
//   
//        Als iteratieve methode voor het oplossen van de matrix- 
//        vergelijking gebruiken we de geconjugeerde gradient methode. 
//        Er wordt een oplossing van de 
//        vergelijking Ax=B bepaald, zodanig dat de maximum-norm van 
//        het residu met een factor tol wordt verkleind. 
//   
//  =================================================================== 
//           De preconditionering. 
//  =================================================================== 
//   
//        Er wordt een standaard preconditionering gemaakt waarbij 
//        alleen de hoofddiagonaal aangepast hoeft te worden. Dit 
//        maakt een efficiente implementatie mogelijk zoals is 
//        voorgesteld door Eisenstat. 
//        De hoofddiagonaal van de preconditionering wordt 
//        aangepast zoals is voorgesteld door Gustafsson. Dit 
//                                          T 
//        betekent dat er een splitsing A=LL- R wordt gemaakt 
//        zodanig dat de rijsommen van de restmatrix R gelijk zijn 
//        aan nul. De incomplete ontbinding is in dus exact 
//                                           T 
//        voor een constante vector e:  Ae=LL e. 
//   
//        Er wordt bovendien een symmetrische schaling uitgevoerd zodanig 
//        dat de hoofddiagonaal van de 
//        preconditionering gelijk is aan I. 
//    !!  Het berekenen van de preconditionering is sterk 
//    !!  recursief van karakter. 
//    !!  Wanneer bij tijdsafhankelijke problemen de matrix slechts 
//    !!  weinig varieert per tijdstap, is het daarom raadzaam om 
//    !!  de preconditionering gedurende enkele tijdstappen constant 
//    !!  te houden. 
//   
//  =================================================================== 
//           De keuze van de parameter alfa 
//  =================================================================== 
//   
//        Bij problemen met veel Neumann-randvoorwaarden is het vaak 
//        gunstig om alfa iets groter dan 1 te kiezen. 
//        In de huidige implementatie wordt alf gelijk gekozen aan 
//         1+10/(N*M) 
//   
//   
//  =================================================================== 
//               De opslag van de matrix 
//  =================================================================== 
//   
//        De opslag van A wordt duidelijk via de implementatie van 
//        de matrixvectorvermenigvuldiging Q=A(P). Er is hierbij 
//        aangenomen dat op de randen overal 'dummy-componenten' 
//        aanwezig zijn, zodat If-statements zoveel mogelijk voorkomen 
//        kunnen worden. 
//   
//             FOR i=1 TO N DO 
//              FOR j=1 TO M DO 
//               Q(i,j)=a1(i,j)*P(i-1,j)+m_a2(i,j)*P(i,j-1)+m_a3(i,j)*P(i,j)+ 
//            *       m_a2(i,j+1)*P(i,j+1)+a1(i+1,j)*P(i+1,j) 
//   
//        De opslag van de preconditionering wordt duidelijk met de code 
//                                T 
//        voor het oplossen van LL v=v: 
//   
//            DO 3110 i=1,N 
//              DO 3111 j=1,M 
//                V(i,j)=V(i,j)-m_a2(i,j)*V(i,j-1)-a1(i,j)*V(i-1,j) 
//   3111       CONTINUE 
//   3110     CONTINUE 
//            DO 3120 i=N,1,-1 
//              DO 3121 j=M,1,-1 
//                V(i,j)=V(i,j)-m_a2(i,j+1)*V(i,j+1)-a1(i+1,j)*V(i+1,j) 
//   3121       CONTINUE 
//   3120     CONTINUE 
//   
//        B bevat het rechterlid van de matrixvergelijking die wordt 
//        opgelost. De oplossing komt in X terecht. 
//   
//        Z en Az zijn hulpvariabelen die nodig zijn voor het 
//        toepassen van CG. 
//   
//        D is een hulparray die nodig is om te onthouden welke 
//        schaling er is uitgevoerd. 
//   
//        Start, finish en rij2 zijn nodig voor de tijdmeting. 
//   
//        Als stopcriterium eisen we dat de maximum-norm van het residu 
//        kleiner moet zijn dan tol. 
//        Het CG-algoritme is geimplementeerd zoals dit is beschreven in 
//        het dictaat van Botta (blz 73).} 
//   

/// constructor
Cg4Solver::Cg4Solver(Array2D<REAL> *pmatC, ///< central array
		Array2D<REAL> *pmatS, ///< south array
		Array2D<REAL> *pmatW, ///< west array
		int Nx1, ///< width
		int Nx2, ///< height
		REAL tolerance) ///< solver tolerance
:
		base(Nx1, Nx2, tolerance), m_a1(*pmatW), m_a2(*pmatS), m_a3(*pmatC), m_D(
				COLUMNS, ROWS), m_Y(COLUMNS, ROWS), m_Z(COLUMNS, ROWS), m_Az(
				COLUMNS, ROWS) {
	ASSERT(pmatC->dim1() == COLUMNS && pmatC->dim2() == ROWS,
			"c array size mismatch");ASSERT(
			pmatS->dim1() == COLUMNS && pmatS->dim2() == ROWS,
			"s array size mismatch");ASSERT(
			pmatW->dim1() == COLUMNS && pmatW->dim2() == ROWS,
			"w array size mismatch");
	preconditioner();
}

/// destructor
Cg4Solver::~Cg4Solver() {
}

/// solves system
void Cg4Solver::solve(Array2D<REAL> *pB, ///< pointer to right hand side
		Array2D<REAL> *pX) ///< pointer to X's
		{
	// this is done inside the WavesComputer::compute() parallelization
#ifdef _OPENMP
#pragma omp master
#endif
	{ //------------------------------------------------------------------------
		int i, j, tel;
		REAL rr, rr0, alf, bet, rrn, zAz, rcrit;

		Array2D<REAL> &B = *pB;
		Array2D<REAL> &X = *pX;

		// Initialiseer tol, Az, Z en Y
		REAL tol = m_tolerance;
		m_Az = 0.0;
		m_Z = 0.0;
		m_Y = 0.0;

		//  Vermenigvuldig het rechterlid B met D
		for (j = 1; j <= m_Nx2; ++j) {
			for (i = 1; i <= m_Nx1; ++i) {
				B[i][j] *= m_D[i][j];
			}
		}

		// Bereken de norm van B
		rr0 = 0.0;
		for (j = 1; j <= m_Nx2; ++j) {
			for (i = 1; i <= m_Nx1; ++i) {
				rr0 += FloatUtils::square(B[i][j]);
			}
		}

		// Afbreekcriterium
		rcrit = (rr0 + 1.0) * FloatUtils::square(tol);

		// In onderstaand statement wordt R=B-A(X) uitgevoerd.
		for (j = 1; j <= m_Nx2; ++j) {
			for (i = 1; i <= m_Nx1; ++i) {
				X[i][j] /= m_D[i][j];
			}
		}
		for (j = 1; j <= m_Nx2; ++j) {
			for (i = 1; i <= m_Nx1; ++i) {
				B[i][j] = B[i][j] - m_a1[i][j] * X[i - 1][j]
						- m_a2[i][j] * X[i][j - 1]
						- (m_a3[i][j] + 2.0) * X[i][j]
						- m_a1[i + 1][j] * X[i + 1][j]
						- m_a2[i][j + 1] * X[i][j + 1];
			}
		}

		// B wordt opgelost uit LB=B
		for (j = 1; j <= m_Nx2; ++j) {
			for (i = 1; i <= m_Nx1; ++i) {
				B[i][j] = B[i][j] - m_a2[i][j] * B[i][j - 1]
						- m_a1[i][j] * B[i - 1][j];
			}
		}
		for (j = 1; j <= m_Nx2; ++j) {
			for (i = 1; i <= m_Nx1; ++i) {
				m_Az[i][j] = X[i][j];
			}
		}
		for (j = 1; j <= m_Nx2; ++j) {
			for (i = 1; i <= m_Nx1; ++i) {
				X[i][j] = m_Az[i][j] + m_a1[i + 1][j] * m_Az[i + 1][j]
						+ m_a2[i][j + 1] * m_Az[i][j + 1];
			}
		}

		// z0=r0
		rr = 0.0;
		for (j = 1; j <= m_Nx2; ++j) {
			for (i = 1; i <= m_Nx1; ++i) {
				m_Z[i][j] = B[i][j];
				rr += FloatUtils::square(B[i][j]);
			}
		}
		tel = 0;
		if (rr >= 1e-60) {
			tol = rr * FloatUtils::square(tol);
			do {
				++tel;
				// In de onderstaande 2 loops wordt de vector Az gelijk gemaakt aan A maal Z.
				// Dit wordt op efficiente manier geimplementeerd, zoals is voorgesteld door Eisenstat.
				for (j = m_Nx2; j >= 1; --j) {
					for (i = m_Nx1; i >= 1; --i) {
						m_Y[i][j] = m_Z[i][j] - m_a2[i][j + 1] * m_Y[i][j + 1]
								- m_a1[i + 1][j] * m_Y[i + 1][j];
					}
				}
				for (j = 1; j <= m_Nx2; ++j) {
					for (i = 1; i <= m_Nx1; ++i) {
						m_Az[i][j] = m_Z[i][j] + m_a3[i][j] * m_Y[i][j]
								- m_a1[i][j] * m_Az[i - 1][j]
								- m_a2[i][j] * m_Az[i][j - 1];
					}
				}

				// zAz=inpr(Z,Az)
				zAz = 0.0;
				for (j = 1; j <= m_Nx2; ++j) {
					for (i = 1; i <= m_Nx1; ++i) {
						m_Az[i][j] += m_Y[i][j];
						zAz += m_Z[i][j] * m_Az[i][j];
					}
				}
				alf = rr / zAz;

				// rrn=inpr(B,B)
				rrn = 0.0;
				for (j = 1; j <= m_Nx2; ++j) {
					for (i = 1; i <= m_Nx1; ++i) {
						X[i][j] += alf * m_Z[i][j];
						B[i][j] -= alf * m_Az[i][j];
						rrn += FloatUtils::square(B[i][j]);
					}
				}
				bet = rrn / rr;
				rr = rrn;

				// (Z(N+1)=B(N+1)+bet*Z(N)
				for (j = 1; j <= m_Nx2; ++j) {
					for (i = 1; i <= m_Nx1; ++i) {
						m_Z[i][j] = B[i][j] + bet * m_Z[i][j];
					}
				}
				rr = rrn;
			} while (rrn >= rcrit);
		}

		// Tenslotte wordt X opgelost uit L  X=X
		for (j = m_Nx2; j >= 1; --j) {
			for (i = m_Nx1; i >= 1; --i) {
				X[i][j] = X[i][j] - m_a1[i + 1][j] * X[i + 1][j]
						- m_a2[i][j + 1] * X[i][j + 1];
			}
		}
		for (j = 1; j <= m_Nx2; ++j) {
			for (i = 1; i <= m_Nx1; ++i) {
				X[i][j] *= m_D[i][j];
			}
		}
	} //single--------------------------------------------------------------
#ifdef _OPENMP
#pragma omp barrier
#endif
} // solve 

/// preconditions matrices
void Cg4Solver::preconditioner() {
	int j, i;

	m_D = 1.0;

	// De preconditionering en schaling bepalen:
	REAL alf = 1.0 + 10.0 / (REAL) (m_Nx1 * m_Nx2);
	for (j = 1; j <= m_Nx2; ++j) {
		for (i = 1; i <= m_Nx1; ++i) {
			m_D[i][j] = m_a3[i][j] * alf
					- m_a1[i][j] * (m_a1[i][j] + m_a2[i - 1][j + 1])
							/ m_D[i - 1][j]
					- m_a2[i][j] * (m_a2[i][j] + m_a1[i + 1][j - 1])
							/ m_D[i][j - 1];
		}
	}

	// Er wordt een symmetrische schaling uitgevoerd zodanig dat de hoofddiagonaal van L gelijk is aan I.
	for (j = 1; j <= m_Nx2; ++j) {
		for (i = 1; i <= m_Nx1; ++i) {
			if (m_D[i][j] <= 0.0) // check on positiveness of matrix
					{
				ASSERTFALSE();
			}
		}
	}

	// Om de efficiente implementatie van Eisenstat toe te kunnen passen,
	// wordt niet de hoofddiagonaal m_a3 zelf, maar m_a3-2I opgeslagen.
	for (j = 1; j <= m_Nx2; ++j) {
		for (i = 1; i <= m_Nx1; ++i) {
			m_a3[i][j] = m_a3[i][j] / m_D[i][j] - 2.0;
			m_D[i][j] = 1.0 / sqrt(m_D[i][j]);
			m_a1[i][j] = m_a1[i][j] * m_D[i - 1][j] * m_D[i][j];
			m_a2[i][j] = m_a2[i][j] * m_D[i][j - 1] * m_D[i][j];
		}
	}

} // preconditioner
